#include "Encoder.h"
#include "PosSpdFeedback.h"


#define D3(x)  ((x)*1000)


// Max number of loop to clear before overflow. For s32d3 position, it's assumed that 
// 2^31 = LOOP_MAX * ReductionRatio * D3, where LOOP_MAX is assigend 10 bits.
#define LOOP_MAX D3(1024)


/**
@brief read position and speed
@param enc[in] The Enocder object.
@param s32d3Pos[out] position, in unit of loop
@param s32d3Spd[out] speed, in unit of rev/sec
@param dt_milli_sec The interval between now and last time this function is called.
*/
void Spd_ReadPosSpdD3(PosSpd_TypeDef* pstPosSped, Enc_TypeDef* pstEnc, int32_t *s32d3Spd, int32_t dt_milli_sec) {    
    // ===== read position =====
    pstPosSped->s32d3EncLoopsOld = pstPosSped->s32d3EncLoopsCur;
    Enc_GetPosD3(pstEnc, (int32_t *)&(pstPosSped->s32d3EncLoopsCur));
    
    // ===== calculate speed =====
    *s32d3Spd = (pstPosSped->s32d3EncLoopsCur - pstPosSped->s32d3EncLoopsOld) / dt_milli_sec;
    *s32d3Spd *= 1000;  // keep d3 format
    
    // ===== reset to zero pos before overflow/underflow =====
    if (LOOP_MAX < pstPosSped->s32d3EncLoopsCur) {
        pstPosSped->s32d3EncLoopsCur -= LOOP_MAX;
        pstPosSped->s32d3EncLoopsOld -= LOOP_MAX;
        Enc_ResetLoops(pstEnc);
    }
    else if ((-LOOP_MAX) > pstPosSped->s32d3EncLoopsCur){
        pstPosSped->s32d3EncLoopsCur += LOOP_MAX;
        pstPosSped->s32d3EncLoopsOld += LOOP_MAX;
        Enc_ResetLoops(pstEnc);
    }
}


/**
@brief Read position and speed, return the number of pulses
@param enc[in] The Enocder object.
@param s32Pos[out] position, in unit of timer's count (number of pulses)
@param s32d3Spd[out] speed, in unit of pulses/sec
@param dt_milli_sec The interval between now and last time this function is called.
*/
//void Spd_ReadPosSpdCnt(Enc_TypeDef* pstEnc, int32_t *s32Pos, int32_t *s32d3Spd, int32_t dt_milli_sec) {
//    int32_t _s32Cnt = Enc_GetPulsCnt(pstEnc);
//    
//    // reset position, so every time the position we get represents a delta of position.
//    Enc_ClearLoopsPulses(pstEnc);
//    
//    *s32Pos += _s32Cnt;
//    *s32d3Spd = 1000L * 1000L * _s32Cnt / dt_milli_sec;
//}


